import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32
import sys

class ResultPublisherNode(Node):
    def __init__(self):
        super().__init__('result_publisher_node')
        self.current_value_ = 0
        # 创建发布者
        self.publisher_ = self.create_publisher(Int32, 'result2_topic', 10)
        self.get_logger().info('Publisher for /result2_topic created.')

        # 创建一个用于切换值的定时器，每 5 秒触发一次
        switch_period = 12.0  # seconds
        self.switch_timer = self.create_timer(switch_period, self.switch_value_callback)
        self.get_logger().info(f'Value switching every {switch_period} seconds.')

        # 创建一个用于持续发布的定时器，例如每 0.1 秒触发一次 (10 Hz)
        publish_period = 0.1 # seconds
        self.publish_timer = self.create_timer(publish_period, self.publish_timer_callback)
        self.get_logger().info(f'Publishing current value every {publish_period} seconds.')

    def publish_timer_callback(self):
        """持续发布当前值"""
        message = Int32()
        message.data = self.current_value_
        self.publisher_.publish(message)

    def switch_value_callback(self):
        """切换要发布的值"""
        # 在 0 和 1 之间切换
        self.current_value_ = 1 - self.current_value_
        self.get_logger().info(f'Switching value to: {self.current_value_}')


def main(args=None):
    rclpy.init(args=args)
    try:
        node = ResultPublisherNode()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        # 销毁节点并关闭rclpy
        if 'node' in locals() and rclpy.ok(): # 检查 node 是否已定义
            node.destroy_node()
        if rclpy.ok(): # 检查 rclpy 是否仍在运行
            rclpy.shutdown()

if __name__ == '__main__':
    main()
